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ABSTRACT 


The performance of a sensor-blending scheme for two different bandwidth sen- 
sors is significantly improved when a Kalman filter is used to blend the outputs 
vice classical control methods. This Kalman filter signal blender is designed and 
implemented in a computer program developed for this thesis. Several tracking sce- 
narios are simulated and analyzed. These scenarios are representative of the input 


expected into the sensors on a Space Based Laser. 
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I. INTRODUCTION 


A. GENERAL 

The weapons being developed for the Strategic Defense Initiative require un- 
precedented pointing accuracies. For the case of the Space Based Laser (SBL) and 
Neutral Particle Beam, the pointing accuracy required is analogous to hitting a 
beach ball on the Empire State Building with a laser on Pike's Peak in Colorado. 
The problem does not end with being able to hit the beach ball: the laser has to illu- 
minate the target for a specified period of time. The United States Army Strategic 
Defense Command has a precision pointing test bed located near Denver, Colorado. 
This facility is operated by the Martin Marietta Corporation. The test bed facility. 
known as the Rapid Retargeting/Precision Pointing (R2P2) facility is the vehicle 
through which the technologies required for the high pointing accuracies and rapid 
retargeting are being developed and tested. The R2P2 facility is currently config- 
ured to simulate the Space Based Laser, the inertial reference unit and the various 
other SBL components. 

The heart of the R2P2 facility and the SBL is the fine pointing system. The fine 
pointing system’s mission is to keep the line of sight of the weapon system pointed 
at the target. Steering mirrors are used to control the inertial line of sight angle. 
The error signal received by the steering mirrors can be treated as the difference of 
two signals, target position command angle (0 < f < 0.5 Hz) minus the line of sight 


feedback angle (0 < f < 40Hz). The angles include disturbances such as command 


vehic.e motion and beam expander structural vibration. The steering mirrors must 








track the low frequency target and filter the high- and low-frequency disturbances 


from the line of sight. 

Presently, the low frequency portion of the steering mirror error signal is pro- 
vided by the Alignment Inertial Reference (AIR) platform, Figure 1.1. Due to the 
nature of the application and the type of sensor, the fine tracker operates at a low 
sampling rate and cannot provide high frequency information. The proposed con- 
cept is to use the AIR platform as a pseudo target, or cooperative target. It provides 
a mirrored surface pointed at the target and located on the weapons system. An 
alignment system marker beam is reflected by this surface and a sensor, other than 
the fine tracker, is used to obtain line of sight information. This alignment sensor 
does not have the low sample rate restriction and can be used to obtain high fre- 
quency information. The command signal for the AIR platform is formed by a sum 
of signals from the fine tracker, the alignment sensor and the AIR platform angle 
sensor. 

The Strategic Defense Command and Martin Marietta desire an alternative 
approach for the fine pointing system on R2P2. The improvement, Figure 1.2, 
involves eliminating the AIR platform from the loop. Low frequency target data is 
obtained from the fine tracker, which samples at 50 Hz. A second signal is formed 
by blending the output from two sensors that measure the beam expan.ier angle. 
a strap-down gyroscope and a magneto-hydrodynamic (MHD) angular vibration 
sensor. The strap-down gyro yields low frequency information while the MHD is 
designed to give high frequency observations. The difficulty witi this scheme is 
the blending of the two signals to produce a broad-band measurement of the beam 
expander angle. The output of the alignment sensor is subtracted from the output of 


the signal mixer to yield a high frequency line of sight angle measurement. A second 








signal mixing network combines this signal with the low frequency information from 


the fine tracker. 


This research project focused on the mixing of the measurements from the two 


sensors, the gyroscope and the MHD, in an effort to fulfill the stated requirements. 


Those requirements, put forth by Martin Marietta, were: 


i) 


. Extremely accurate tracking of input signal. 
. Extremely fast lock on time, 20 ms or better. 


. Flatness in magnitude and phase for the combined low pass and high pass 


sensors as shown in Figure 1.3. 


. Steep cutoff rates for the outputs of the individual compensating filters, to 


minimize noise contributions from the individual sensors in their non-valid 


regions of measurement. 


. A selectable blending frequency, selectable at any point between w,, and wy in 


order to blend the sensors for minimum noise. 


. Minimal sensitivity of the compensating network to parameter variation in the 
sensors. 
. Minimum number of poles and minimum DC gain in the compensating filters. 


To meet these requirements, a Kalman filter was designed to ‘nix the outputs 


from the two sensors. The Kalman predicts the states of the sensors, discarding the 


noise, based on previous measurements. The results should be the correct frequency 


response and an extremely accurate tracking of 6gx. The results of the Kalman 


filter signal blending will be compared with the signal blending filter scheme that 
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Figure 1.3: Desired Frequency Response for System 


Martin Marietta has devised, utilizing classic filter design. Different sensor types 


will be compared with these results in an effort to further improve the design. 





II. PROBLEM STATEMENT 


A. GENERAL 


The filter used involves two sensors with different bandwidths, measuring a 


common input. The filter then blends the two inputs using Kalman techniques. 


The problem was developed using state space methods. Given the noise clut- 


tered input angle, 9, we are interested in the noise-free measurement of this angle 


over a broad band of frequencies. The state variables, (rl. r2, 13. rt, x5), for this 


plant are 0, 9c, 9c, Bay. and Oxy as defined in Table 2.1. 


TABLE 2.1: STATE VARIABLE DEFINITIONS 


zl 


z2 


x3 


r4 


B. SYSTEM MODEL 


True state to be tracked 90 
Gyroscope angle 

Gyroscope angle rate 

MHD angle 


MHD angle rate 


The system to be modeled in this problem is that of an inertial reference unit 


on the Space Based Laser. In the development of this work, the assumption was 


made that all noise encountered is white noise. 





The simplified block diagram for the system is 





Figure 2.1: Simplified Block Diagram for Sensors 


The transfer functions for the two sensors were given by Martin Marietta [Ref. 1] 


from manufacturer data and testing. The gyroscope’s transfer function is 


3947.8 
H = 2. 
u(s) = F738 Bits 13017 8 el 
The MHD transfer function ts 
“(3+2 
H,(s) = coe (2.2) 


s? + 12.57s + 157.91 
Figure 2.2 shows the frequency response of both sensors. It can be seen that both 


sensors are second-order systems. 


The continuous state space equations for the modeled system are 


t= Ac+ Bw (2.3) 


Magnitude, (db) 
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50 
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Figure 2.2: Frequency Response of Both Sensors 
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where 


e B = input driving function matrix 
e C = measurement matrix 
system noise matrix 


@ v = sensor noise, 


and the state vector is 
7 
9G 
8G 
Ont 
Oss 


Using Equations 2.1 and 2.2 and the requirements for the phase and magnitude of 


the output, the A matrix can be formed as 


where 
e@ wc = gyro cutoff frequency 
e wy = MHD cutoff frequency 


e ¢ = damping coefficient for each sensor 








For the model, it is desired that the fastest reaction time possible is achieved. 
To do this, the system is critically damped, ¢ = 1. The cutoff frequencies come from 
sensor specifications and testing. Equations 2.1 and 2.2 reflect the cutoff frequencies 
and damping coefficient values given. The classical 2"? order damped system has 


the form 


w? 


AK.) = —-——--—— 
(s) s?+2¢w + w? 


Discretizing the state equations yields the following discrete state space equa- 
tions, 


Teep = Or, + Aw, (2.8) 


where 
@ ry, = parameter to be estimated (State Vector). 


e ¢ = state transition matrix which describes how the states of the dynamic 


system are related. 
e A = state transition matrix for input driving function. 
® w, = system noise matrix. 


From Equation 2.8 and the above assumptions, the ¢ matrix is 


1 0 0 0 0 
4.83 x 1074 0.999 4.854 x 1074 0 0 
g= 1.913 —1.913 0.9886 0 0 Le 
1.966 x 107° 0 0 0.999 4.965 x 1074 
7.846 x 107? 0 0 —7.846 x 107? 0.9875 


(2.9) 
The system noise for the model comes from the input that the sensors are measuring. 
This input will have noise from the vehicle, the mirrors and the beam expander. This 


noise was modelled in accordance with the R2P2 observations by Martin Marietta. 


Il 





C. MEASUREMENT MODEL 


For a linear measurement process, the measurements are linearly related to the 
state variables and can be modeled using the discrete linear measurement equation 
from Equation 2.4, 


pS Hp + yy (2.10) 


where 


z, = set of measurements 


e H = observation matrix that gives the relationship between the measurements 


and the state vector 
@ x, = state vector 


@ v, = measurement noise from the sensors 


With the appropriate values for H, Equation 2.10 becomes 
0100 0 
2, = 2: 
k i 001 | [sete (2.11) 


In this blending problem, the measurements are made of the beam expander 
by the sensors that make up the inertial reference unit. The measurements are made 
noisy by the noise inherent in the sensors. The sensors have been rigorously tested 
and the power spectral densities have been computed by Martin Marietta. Figure 
2.3 shows the computed noise spectra for the two sensors. 

The noise from the sensors is a function of many variables including tem- 
perature and bandwidth to be measured. Although this is generally a non-white, 
non-gaussian noise process, it can be adequately described as a white noise process 


over an extended period of time. 
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Figure 2.3: Noise Spectral Densities for Both Sensors 








The state and measurement equations are now ready to be implemented in the 


simulation. What follows is the development of the Kalman filter equations that are 


the heart of this exercise. 





III. KALMAN FILTER THEORY 


A. GENERAL 

Filtering refers to the process of estimating the state vector at the current time, 
based upon all past measurements. An optimal filter concentrates on optimizing a 
specific performance measure used to approximate the quality of the estimate. The 
Kaiman filter is the optimal filter in a class of linear filters that minimize the mean 
square estimation error between actual and desired output. In other words, the 
Kalman filter attempts to minimize the elements along the main diagonal of the 
state error covariance matrix. The Kalman filter has been used extensively in the 
design of estimation models since it was first presented by Kalman and Bucy [Ref. 
5] in 1960. The filter itself is actually a recursive algorithm for processing discrete 
measurements or observations in an optimal manner. [Ref. 6:p. 104] A priori 
knowledge of the state estimate and its error covariance, and the current observation 
is required. The Kalman filter is a useful algorithm when both the system model 
and the measurement model are linear functions of the state variables and these 


models can be described by the equations 


Leg p = Oper, + Avy C30} 


k= Hr, + Uz (3.2) 


B. SYSTEM MODEL 


The state space model of the system is given by Equation 3.1 and the mea- 


surements are described by Equation 3.2. This is a standard state space matrix 








representation for a system of linear differential equations. In Fyuation 3.1. rg rep- 


resents the physical state and ry,,, represents the next state of the discrete system. 

The values 6 and \ represent the discrete time state matrices. The value of 
r, is the true observed parameters of the state and ty, and uw, are observation noise 
and state expectation noise, respectively. 

This system is time invariant since neither ¢ nor H is dependent on time. 
The noise processes are considered to be stationary, independent, white gécssian 
noise with zero mean. This assumes that white noise is an idealization of nature's 
true state: however, it is an extremely good approximation for many systems. The 


statistical properties of the noise are given below. 


E [uy] = 0 (3.3) 

E [w, wz] = 6;. (3.4) 
E [vy] = 0 (3.3) 

E [ref] = Rb;% (3.6) 
E |wieg] = 0 (3.7) 


The matrices Q and R in Equations 3.4 and 3.6 are the covariance matrices 
for the noise processes. For this system, the noise covariance matrices are non-zero 
diagonal matrices, which denote the power present in the noise. This mode] will be 


further discussed in Chapter 4. 


C. LINEAR RECURSIVE FORM 

Before deriving the filter equations, the form of the filter must first be deter- 
mined. The form assumed for most Kalman filters is shown in Equations 3.8 and 
3.9. 


Teak = MPak (3.8) 
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Teeijker = Kotegije + Ag 2K (3.9) 


The current estimate, 24411441, 18 a linear combination of the previous estimate, 
Fe4ije, and the current observation. z%41. This form is chosen for its simplicity. but 


Reference 4 demonstrates it is optimal for a linear system. 


D. ERROR COVARIANCE 


The error covariance matrices are described by Equations 3.10 and 3.11, 
Presse = E [fesse bri] (3.10) 


ey -T ‘ 
Preijkti = E mere sorrer| (3.11) 
These matrices give a feeling for the expected magnitude of the estimation error. 
Their derivation can be found in Reference 6:p. 108. The Kalman equations begin 


to take shape when Equations 3.10 and 3.11 are combined, 
Praite = Pyro? +Q ; (3.12) 


Additionally, writing Equation 3.11 and incorporating the equations found in Ref- 


erence 2 in the development of the covariance matrix. we get 
Prien: = (1 = GH) Pegg (1 - GH) + GRGT (3.13) 


where G is the Kalman gain matrix. All that remains is to find the value of this 


Kalman Gain matrix. 


E. RESIDUAL AND VARIANCE 
The definition of the residual will be helpful in simplifying the notation re- 
quired for the remainder of the proof. The basis for the residual and its variance 


came from conversations with Steve Spehn [Ref. 7]. The residual is given by 


Teo = Zeer — E [ze51] (3.14) 
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Since the estimate is unbiased, we see that 
E lzegi) = E[Hregi] + E [ves] (3.15) 
E [ze41] = Aegis (3.16) 
By substitution and algebra, we get the final form of the residual, 
reat = Nigga + Ves (3.17) 


(This derivation is from Reference 7.) 


The covariance of the residual is found to be 
tar (rig) =k [rewire] (3.18) 
var [regi] = H Peay +R . (3.19) 
Using the definition of the residual, the observation update equation can be written 
as 
Teper = Pegile + Greg (3.20) 
The Kalman Gain equations can now be derived. 
F. KALMAN GAINS 
Solving Equation 3.13 for G gives, 


-1 
Gar = Pray? (H Presi? + R) (3.21) 


Recognizing the form of the equation in parenthesis to be that of Equation 3.19. we 


simplify to the final form. 
Gear = Prayer’ var [regi]! (3.22) 
Using techniques developed in Reference 3, we simplify Equation 3.13 to 


Praije+i = Bs rect G41] Prati (3.23) 
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G. KALMAN FILTER EQUATIONS 
This derivation has provided a set of recursive equations. which give a time- 
varying optimal gain matrix and a detected error analysis of the estimate. The 


Kalman filter equations are given below. 


Terie = OF ee (3.24) 

Pri, = OPO? + Q (3.25) 

Gast = Perel? (H Pes? + R) (3.26) 
Lepifker = Lege + Ges (cast = Hina) (3.27) 
Prsipegt = (1 -— Gear) Praite (3.28) 


These equations can be further simplified using the definitions of the residual and 
covariance of the residual. This simplication will be incorporated in the simulations. 
Since the Kalman equations are recursive, they are readily adaptable to computer 


simulation. All that is required are the initial conditions: 


Fojo. Initial estimate 


Poo, Error covariance. 


This a priori knowledge is essential to the Kalman process. 

The Kalman equations are now ready to be implemented in estimating a nor- 
mal system. The next step is to make the Kalman adaptable thereby increasing its 
bandwidth. This will be accomplished by adding maneuver gating to the Kalman 


filter. 
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IV. MANEUVER GATING 


A. MAHALANOBIS DISTANCE 

The Mahalanobis distance (MD) is a measure of the derivation of the obser- 
vation from the estimate. The derivation of the MD is found in Reference 8. The 
idea for this procedure was derived from Reference 7. 

The Mahalanobis distance is found using the values for the residual and co- 


variance of the residual, Equations 3.14 and 3.19, 


MD =rf,,var [regi | reas (4.1) 


The resulting scalar is compared with a desired threshold in the program. This 
threshold was picked at MD = 4, which corresponds to the statistical 20 point for 


the noise processes. 


B. RESIDUAL GATING 

Residual gating is the process by which the Kalman adapts itself to large jumps 
in the observation. The system being tracked in this simulation can be expected 
to have large, nearly step-shaped, changes in the observations being tracked. (The 
following derivation comes from Reference 7.) 

A normal Kalman filter would observe this jump and initially considers it as 
a noise perturbation. The Kalman will therefore ignore the Jump, for several steps. 
If the large value persists, the filter will begin to react with speed dependent upon 
the value of the covariance matrix, P, at the time. 

This reaction, although a great benefit for slow-moving tracking situations, is 


extremely restrictive for this system. The requirement for lock-on in 20 milliseconds 
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demands a more proactive Kalman filter. Residual gating provides this proactive 


behavior. 

Residual gating uses the Mahalanobis Distance derived earlier as the “gate” 
for the incrementation of the covariance matrix. There are two ways for a Kalman 
filter to adapt, either by increasing the gain G44, or the covariance matrix, Py. The 
covariance matrix was selected as the means for adaptation. The gate is set up using 


the 2o value discussed earlier. A value of 
MD>4 (4.2) 


results in the observation falling outside the gate and begins the adaptive incre- 
menting of P, 


Prue = FP ee (4.3) 


The constant, F, was used to adaptively increase the last value of the covariance 
matrix, Py. The value of F was derived experimentally to obtain a value that 
results in optimal filter performance. F was found to cause little variance over a 
wide range of values. 

Through analysis, it was decided to use a gating reset of Myo. This results in 
some lag time in the filter, which is made up for by its faster lock-on time. 

The next step in the design is to simulate the inputs and scenarios the system 
will see. What follows are the various simulations used to tes: the filter's ability to 


meet system requirements. 











V. SIMULATIONS 


A. SCENARIOS 
Several scenarios were developed for this simulation to test its applicability to 
the sensor blending problem. In all scenarios, observation noise was present. State 
excitation noise was varied. 
1. Scenario One 
This scenario introduced a | Hz square wave with various noise levels into 
the system. Figure 5.1 shows the input wave. 
2. Scenario Two 
This scenario introduced a 10 Hz square wave into the system with various 
noise levels. See Figure 5.2. 
3. Scenario Three 
This scenario introduced a 50 Hz square wave into the system with various 
noise levels. See Figure 5.3. 
4. Scenario Four 
The input for this scenario is a 100 H[z square wave. This input is the 


high limit provided by Martin Marietta. (Ref. 1] See Figure 5.4. 


B. NOISE INPUTS 

The noise inputs for the model were developed from input previded by Martin 
Marietta (Ref. 1]. Figure 2.3 shows the noise spectral power values for the two 
sensors, MHD and Gyro. The values used throughout the simulations for the sensor 
noises were taken as the median from the graphs. The values were entered as vg 


and vay. after conversion. 


to 





The values entered for state excitation noise, w,, were derived from the ex- 
pected range of the fine tracking system. Varying the level of 1, enables one to test 
the robustness of the model and filter. ‘he mean noise level was selected as 107° 


rad. 


C. RESIDUAL GATING 

A test case was run for Scenario One input without residual gating. Figure 5.5 
shows the results of a normal Kalman filter without residual gating. As can be scen, 
the performance is unacceptable for the accuracy requirements stated. It will serve 


as a good reference for the Kalman filter used in the renainder of the simalations, 
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Figure 5.1: Scenario One 
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Figure 5.2: Scenario Two 


D. REQUIREMENT FOR FREQUENCY RESPONSE 
The sponsor of this thesis, Martin Marietta, requested a frequency response of 
the filtered system as part of their specifications [Ref. 2]. The Bode plot developed 


from the model is a result of this requirement. 


E. BODE FORMULATION 

A bode plot is a plot of a system transfer functions response over a range of 
frequencies. Martin Marietta desired a unity gain frequency response over the range 
of interested frequencies, 0.01-100 Hz. [Ref. 1] A transfer function was generated us- 
ing steps put forth in Reference 6 for a Wiener steady state optimal filter. A Wiener 
filter is an optimal filter, identical to the Kalman, if the statistics are Gaussian. The 


results of the derivation give a filter transfer function of the form 
H(z) = [(z21- $+ GH)"G| (5.1) 
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Figure 5.3: Scenario Three 


The transfer function was derived using the program in Appendix B. This 
transfer function was combined with the sensor transfer functions, from Figure 6.45, 


and a Bode plot, Figure 6.46, was obtained. 
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Figure 5.4: Scenario Four 
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Figure 5.5: Scenario One, No Residual Gating 
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(1.2 


VI. DISCUSSION OF RESULTS 


A. GENERAL 

All of the simulations conducted were done using an IBM-PC and the software 
language PC-MATLAB. The program codes are contained in Appendices A and B. 
The results achieved could not be shown to completely satisfy the requirements put 
forth by Martin Marietta. Specificially, the frequency response of the steady state 
gain Kalman blended system did not meet the desired specifications. This incon- 
sistency was resolved by the adaptive gating incorporated in the system designed. 


This will be discussed in detail in Section VI-C. 


B. KALMAN PERFORMANCE 

The performance of the Kalman filter was evaluated through several steps of 
increasing noise and frequency of the input. The filter design was for step and square 
wave input, as per Martin Marietta’s guidance [Ref. 1]. The Kalman is a Type 0 
system, by design, so it will not be able to follow a ramp or sinusoid. It can be 
modified to follow those two inputs, but with the penalty of not being a real-time 
system any longer. The system this filter was built for. the R2P2. is extremely 
dependent on real-time results. Therefore, the Kalman was designed to be as fast 
as possible. 

The first simulation conducted was for an input of 5 mrad that crops to 0 mrad 
at 0.05 seconds with Q=0 and R ~ 0. The R matrix could not be made to equal zero 
due to MATLAB constraints. This simulation will act as a baseline for which the 
others will be compared. Figure 6.1 shows the input and filter output. Figure 6.2 


illustrates the error between the actual input and the output of the Kalman. Figure 
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Figure 6.1: Baseline - Y1 Estimation for Model vs. Input 


6.3 shows the value of the mean of the residual over the period of the simulation and 
Figure 6.4 illustrates the ability of the filter to achieve rapid lock-on. The lock-on 
gate used in these simulations is + 20 rad. These graphs are of the first state (11) 
of the system, which is the state we are concerned with following. The no-noise 
input scenario is unrealistic, but is effective in giving a baseline for the rest of the 
analysis. With no noise, the Kalman is able to lock-on to the input in one time 
step. The mean of the residual and lock-on time are two ways of checking Kalman 
effectiveness. They will be used in analyzing the performance of the alman for the 


scenarios. 
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Figure 6.2: Baseline - Plot of Error Between Estimate and Input (V1) 
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Figure 6.3: Baseline - Mean of Error 
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Figure 6.4: Baseline - System Lock-On Time 


C. SCENARIO RESULTS 
1. Scenario One 

Three different runs were made for Scenario One, in which the noise 
inputs were varied. The input signal was a 1 Hz square wave. The first run had the 
state noise covariance, Q, equal to 0 and the measurement noise covariance matrix 
equal to the values obtained from Figure 2.3. Figures 6.5 to 6.8 show the simulation 
results. Figures 6.9 to 6.12 show the results of the next run in which noise was 
introduced into the Q matrix and R ~ 0. Figure 6.13 and 6.16 illustrate the results 
of entering representative noise into both the @ and f matrices. 

As would be expected, the mean of the residual and lock-on times were 
progressively worse for each case. It is also obvious that state measurement noise, 


R, is the dominant noise input in the filter. The results of the final run of this 
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Figure 6.5: X1 Estimation for Model vs. Input 


scenario are well within the desired specifications. The Kalman is locking on with 
little deviation in 10-15 time steps. 
2. Scenario Two 
This scenario takes the basic system and applies a 10 Iz square wave 
input with amplitude of + 5 mrad. The values for Q and /? will remain constant 
for the remainder of the scenarios. Figures 6.17 to 6.20 show the results of entering 
the 10 Hz wave into the Kalman. 
For this input, the Kalman performs exceptionally well Lock-on, Figure 
6.20, occurs in less than 20 time steps and the mean of the residual, Figure 6.19, 
and the error, Figure 6.18, are extremely low. 
3. Scenario Three 
Scenario Three applied a 50 Hz square wave into the Kalman. This 


frequency of input appears to tax the Kalman’s ability to follow an input. Figure 


31 


RADIANS 
i) 


0 0.05 0.1 0.15 0.2 
TIME (sec) 


Figure 6.6: Plot of Error Between Estimate and Input (V1) 
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Figure 6.7: Mean of Error 
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Figure 6.8: System Lock-On Time 
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Figure 6.9: X1 Estimation for Model vs. Input 
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Figure 6.10: Plot of Error Between Estimate and Input (X1) 
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Figure 6.11: Mean of Error 
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Figure 6.13: Xi Estimation for Model vs. Input 
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Figure 6.14: Plot of Error Between Estimate and Input (X1) 
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Figure 6.15: Mean of Error 
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Figure 6.16: System Lock-On Time 
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Figure 6.17: X1 Estimation for Model vs. Input 
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Figure 6.18: Plot of Error Between Estimate and Input (1) 
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Figure 6.19: Mean of Error 
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Figure 6.20: System Lock-On Time 


6.21 shows the Kalman trying to track the input. The error graph, Figure 6.22, 
shows the output gets close to the input very rapidly, but does not lock-on, Figure 
6.24, and stay there. Partial lock-on is achieved, but with the input stepping every 
20 time steps, the Kalman has great difficulty getting the covariance matrix and 
gains down. There appears to be a credible performance by the Kalman at this 
point, but it is pushing its avilities with the present specified sample rate of 2 kIIz. 
4. Scenario Four 

A 100 IIz, 5 mrad square wave was input into the Kalman. This was the 
specified range for the blending filter given by Reference 1. Figures 6.25 and 6.28 
show the Kalman’s inability to follow an input of this high of a frequency. A 100 
Hz wave calls for a step up or down every 10 time steps. In other words, t! e input 
goes through two complete periods in the required lock-on time of 20 msec. As with 


Scenario Three, a much higher sample time is needed for the Kalman to track this 
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Figure 6.21: 1 Estimation for Model vs. Input 
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Figure 6.22: Plot of Error Between Estimate and Input (X1) 
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Figure 6.23: Mean of Error 
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Figure 6.24: System Lock-On Time 
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Figure 6.25: X1 Estimation for Model vs. Input 


frequency of input. The zero mean error, Figure 6.27, results from how the mean is 
computed in the program. The program allows for a settling time of 13 time steps 
after gating. The 100 Hz wave causes the filter to gate every 10 time steps. The 


mean cannot be computed and remains zero. 


D. NOISE VARIATIONS 

In order to verify the filter's insensitivity to noise, Scenario Two was modified 
with various levels of state noise and measurement noise. 

The first simulation decreased the values in the Q matrix by an order of mag- 
nitude. As shown in Figures 6.29 through 6.32, this had little or no effect on the 
outputs when compared to Figures 6.17 to 6.20. 


The next three runs involved varying the 2 matrix. The /? matrix was the noise 


input most easily influenced in the system. Variations in electrical current to the 
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Figure 6.26: Plot of Error Between Estimate and Input (.\1) 
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Figure 6.27: Mean of Error 
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Figure 6.28: System Lock-On Time 


sensors or misalignment of cumponents are two ways that could increase the sensor 
noise. To increase the values in the Q matrix would require a failure somewhere in 
the R2P2 damping mechanisms or, in real life, an impact on the structure in space. 

Therefore, the next three simulations involved increasing the magnitude of the 
noise elements Vg and Vay of the F matrix by factors of 2,5, and 10. The resulting 
graphs are shown in Figures 6.33 through 6.44. The progression of the simulations 
show that the system can handle up to an order of magnitude increase in noise in 
both sensors and still function. Figures 6.41 and 6.44 show that the factor of 10 
increase does push the system to the limits of its desired capabilities. Figures 6.33 
through 6.40 show that the system performs quite well for 2 and 5 times the noise 


input. 
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Figure 6.29: X1 Estimation for Model vs. Input 


E. FREQUENCY RESPONSE 

The final portion of the analysis of the Kalman filtered system was the fre- 
quency response. As stated earlier, a flat response over the interval 0.01-100 Hz 
was desired. The frequency response of Martin Marietta’s classical blending system, 
Figure 6.45, is shown in Figure 6.46. The frequency response for the steady state 
gain Kalman filter is shown in Figure 6.47. The Kalman’s frequency response for 
the steady-state gains does not meet specifications. Due to the adaptive gating de- 
signed into the Kalman filter, it will not reach the steady-state gain values utilized 
in the Wiener development under normal conditions. With any kind of input, the 
gains will be adapting continually. The steady-state Kalman approximation meets 
the required error requirements. The adapting that occurs increases the bandwidth 


to the desired range, thereby fulfilling the requirements. 
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Figure 6.30: Plot of Error Between Estimate and Input (Y1) 
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Figure 6.31: Mean of Error 
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Figure 6.33: X1 Estimation for Model vs. Input 
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Figure 6.34: Plot of Error Between Estimate and Input (.X1) 
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Figure 6.35: Mean Of Error 
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Figure 6.36: System Lock-On Time 
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Figure 6.37: X1 Estimation for Model vs. Input 
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Figure 6.38: Plot of Error Between Estimate and Input (1) 
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Figure 6.39: Mean of Error 
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Figure 6.40: System Lock-On Time 
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Figure 6.41: \1 Estimation for Model vs. Input 
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x1(Q°6 

6 10 

4 wal 

2 4 

0) Niieatetan| 

aa Meee 
I 
-2 | 
A J 
-6 = 
-8 
0 0.05 0.1 0.15 0.2 


TIME(sec) 


Figure 6.43: Mean of Error 
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Figure 6.44: System Lock-On Time 
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Figure 6.46: MMAG Blending Scheme 
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Figure 6.47: KALMAN Blending Scheme (State 1) 
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VII. CONCLUSION 


The Bode diagram for the steady-state Kalman filter clearly shows that a 
steady-state gain filter does not meet the bandwidth requirements for the blender. 
If a steady-state Kalman filter had been used, the blending scheme proposed would 
not have functioned properly. But with an adaptive gate Kalman filter, the signal 
blender achieves the desired bandwidth. This is shown in the various simulations 
conducted. The purpose of an adaptive Kalman is to adapt the bandwidth of the 
system it is estimating. The Bude shown is just an approximation of the Kalman 
filter developed. It is a snapshot at a point in time of the adaptive filter. Devel- 
oping a frequency response for an adaptive Kalman filter is a possibility for further 
research. 

For speed and accuracy, the Kalman is vastly superior to the classical! blending 
scheme. Figure 7.1 shows the results of a 1 Hz square wave input into the Martin 
Marietta system. The results from Scenario One are orders of magnitude better. 

The adaptive gating approach used in this design is very versatile in its ap- 
plication. Since time response was a high priority, this versatility was sacrificed to 
a degree. By adjusting the gate and factor, F, the Kalman filter can be adapted 
to follow any transient input. But, the faster the adaptation, the poorer the noise 
filtering the transient. 

Overall, the Kalman filter is superior to the classical approach to blending 
two signals. For speed and accuracy, it is orders of magnitude better. With a few 


modifications, it can be made to follow any input. 
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Figure 7.1: Transient Response - Classical Blending 








APPENDIX A: MAIN PROGRAM AND 
INPUT FILES 


All of the simulations for this project were run on IBM-PC class computers 
using the matrix manipulation language MATLAB, version 3.5f. This appendix 
contains the source code for all of the functions written in support of this project. 

Only minor programming experience is required to understand these files. 
While MATLAB is similar to Fortran. MATLAB’s control structures are much less 
complex. Comments are started by the percent sign (%) and continue to the end of 
the line. 

To aid the reader in scanning and retyping these functions. each file is started 
on a new page. Although an analysis of the workings uf these files is not necessary 
to understand this report, the curious (or skeptical) reader is highly encouraged to 
examine them closely. 

The author neither claims nor desires to hold any copyright privileges on the 
source code. Written requests for the source code on computer disk should be sent 
either to the author or to Professor Harold A. Titus. Address information can be 
found in the Initial Distribution List at the end of this report. 

All of the files listed in the second section of this appendix provide general 
support for the main files listed in the first section. These -upport files are not 
specific to the simulations run for this report, but can be used for a variety of 


purposes. 


System 


Gyroscope. 
This 


# 

# 

# 

# 

# 

# 

4 

# 

# 

# ‘kmax’, 
# 

# 

# 

# 

# 

# frequencies. 
# 

# 


OP AP oP AP WP OP OP OP OP OP OP DP OP OF OF OF OP OP OP OP OP oP OP Of 


delete bx.met 
delete output6.met 
clear; 


khkkkkke 


program tracks square 


THESISTS.M 


waves of 


HHHSHHHH RRR ARR RRR 
30 JAN 90 


MATLAB Simulation of the Beam Expander Inertial Reference 
Unit for the Rapid Retargeting/Precision Pointing (R2/P2) 


# 

# 

# 

# 

# 

# 

# 
Before running this simulation the length for the # 
simulation in seconds must be defined as the variable # 
and the sampling interval is defined as ‘dt’. # 

The program uses a adaptive gate Kalman filter to # 
blend the output of two different sensors. The sensors # 
are a MagnetoHyrodynamic rate sensor and Singer Rate # 
# 

# 

# 

# 

# 


different 


HAHAHA eHHAA HHA A HARRAH HAAR AA RS 


% delete previous meta file 


INERTIAL REFERENCE UNIT 


Magneto HydroDynamic rate sensor. 


kkk Input 
b = 1000*2*pi; 
wg = 10.0*2*pi; 
wm = 2.0*2*pi; 
zeta = 1.0; 
dt = 0.0005; 
kmax = 0.20/dtt+l1; 
I = eye(5); 
date= 26; 
tWg = le-5; 
Wg = 0; 
tWm = le-5; 
Wm = 0; 
% Rhkkkke Input 
AS = zeros(5,5); 
BS = (0;0;Wg;0;Wm]); 
ZS = zeros(2,5); 
% ake Enter 
% 
AS(1,1) = 0; 
AS(2,3) = 1; 


Constants Rk 


kkikkk 


$ 
% 
% 
% The IRU is made up of two sensors. A strap down gyro and a 
% 
% 
% 


kr 


send break freq 

gyro break freq down 
%MHD break freq up 
damping ratio 


tsample rate for system 


% 


State Matrices For Sensors 


Values Into A Matrix 
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State noise for gyro 


wkkkkek 


initialize matrix at zero 
$B matrix 
initialize observer matrix 


kkk 


AS(3,1) 
AS (3,2) 
AS(3,3) 
AS(4,5) 
AS(5,1) 
AS(5,4) 
AS(5,5) 


wg*2; 
-wg*2; 
-2*zetat*wg;} 
Ly 

wm* 2; 
-wm*2; 
-2*zeta*wm; 


tououou 


| 


ae Enter Values Into Observer Matrix Liadal 


ge oP 3 
* 


ZS(1,2) 
2S (2,4) 
2S (2,5) 
H = ZS ; 


Wo ou 
an) 


ae Build Observer Matrix ae 


kkk Discretize State Equations kak 
phi,del]} = c2da(AS,BS,dat); $Discretize states 


aielal Construct Kalman Filter Equations elialiail 


FP OP CP OP CP WH OH OP OF GP 


ak Build State Noise Error Matrix 


= 
e 
i 


= 0; 

le-2; 
5e-5; 
le-6; 
le-5; 


= 
w 
ou uu 


wl*2*eye(5);3 
O;%le-~-5; 


Le) 
Hout 


** Build Measurement Noise Matrix ** 


avg =.1.237e-6; 
avm = 75.0e-6; 


vg = le-1l0*avg; 

vm = le-10*avm; 

v= {vg vm)’; 

R = zeros(2); $R matrix values 
R(1,1) = vg*2; 

R(2,2) = vm%2; 

% 

%  *t Set Initial Error Covariance Matrix ** 
% 

P = leloO*eye(5); 

PO = P; 

% 

% 

% aae% Build Kalman Equations kak 

% 


= zeros(2,kmax) ; 


< 
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m = zeros(5,kmax); 
tsql; 
% 
for i =1:kmax, 
y(:,i)= H*£(:,i)+v.*rand(v); 


end 

% 

time = zeros(1,kmax); 

time(1) = 0; 

mr = zeros(2,kmax); 

Xhat = zeros(5,kmax); 

Xhat(:,1) = {0 0 0 0 O}'; $Initial estimate of states 
k_wait = 0; 

mean_r = [0;0]; % mean of the residual 


for k=2:kmax; 


Xhat(:,k) = phi*xXhat(:,k-1)+del; %X (kK+1/k) 
while 1 
resid = y(:,k)-H*xXhat(:,k-1); 
vresid = H*P*H’+R; variance of the residual 
md2 = resid’/vresid*resid; tMahalanobis distance 
if md2 < 4, gating check 
break; 
else 
P = PO; $P(k/k) 
k wait = 0; 
mean_r = O*mean_r; 
mr(:,k) = mean_r; 
end 
end 
k_wait = k_wait + 1; 
G = P*(H)’/vresid; %Kalman Gains G(k+1l) 
P = (I-G*H) *P; $P(k/k) 
P = phi*P*(phi) ‘+Q; $P(k+1/k) 
Xhat(:,k)=Xhat(:,k)+G*(y(:,k)-H*Xhat(:,k))? $X(K+1/K+1) 


if k_wait >= 13 
kw = k_wait - 13; 


mean_r = kw/(kwtl)*mean_r + 1/ (kwt+l)*resid; 
mco(:,K) = mean_r; 
end 
time (k)=time(k-1)+dt; 
home,k 
end 
% 
zee Calculate the Error *** 
% 


error = m(1,:)-Xhat(1,:)? 


errg = m(1,:)-Xhat(2,:); 
errm = m(1,:)-Xhat(4,:)? 
% 
g ** Lock on Check ae 
% 
for j=l:kmax, 
if abs(error(j))<= 2e-5, 
lo(j) = 0; 
else 
lo(j) = 1.0; 
end; 
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end; 

subplot(211),plot(time,m(1,:),time,xXhat(1,:),’:/) 

title(’Xl ESTIMATION FOR MODEL VS. INPUT’) 

xlabel(‘TIME (sec)’),ylabel (‘RADIANS’) 

gtext(‘STEP INPUT (-) ’) 

gtext(’FILTER OUTPUT (:) ‘’) 

gtext(({’Noise ’,num2str(q),’ Rad’}) 
subplot(212),plot(time,error),title(’PLOT OF ERROR BETWEEN ESTIMATE AND INPUT (X 
xlabel(’TIME (sec)’),ylabel(’RADIANS’) 

meta output2 

pause 

tplot(time,m(1,:),time,Xhat(2,:),’:7) 

$title(’XHAT 2 INPUT’) 

tplot(time,errg) ,title(’PLOT OF ERROR BETWEEN ESTIMATE AND INPUT X27’) 
%xlabel(’TIME (sec)’),ylabel(’RADIANS’) 

%meta 

tpause 

Splot(time,m(1,:),time,Xhat(4,:),’:/) 

ttitle(’XHAT 4 INPUT’) 

tplot(time,errm) ,title(’PLOT OF ERROR BETWEEN ESTIMATE AND INPUT X4’) 
txlabel(’TIME (sec)’),ylabel(’RADIANS’) 

meta 

plot(time,mr(1,:)),title(’MEAN OF ERROR’) ,xlabel(’TIME’) 

axis({O0 .20 -0.2 1.2)); 

plot(time,lo) ,title(’SYSTEM LOCK ON TIME’) 

xlabel(’TIME (sec) ’) 

meta 

pause 

clg; 

axis({0 .20 -5e-5 5e-5)); 

plot(time,error),title(’PLOT OF ERROR BETWEEN ESTIMATE AND INPUT (X1)’) 
Xlabel TIME (sec)’),ylabel(’RADIANS’) 

meta 

axis; 
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TSQ1.M 


System 


THESISTS.M. 


t=0.0005; 
cmax=0.20/dt+l1; 


2S GL ge of 26 9h cP oP 0 oP FP GP CP FP Of GP OF DF UP OF OF Of 


= zeros(2,kmax); 
= zeros(5,kmax); 


3S of ww 06 


for 1= 1:kmax; 


if i<=100, 
state = [.005 .005 0 .005 0)’; 
else 
state = (0 0 0 0 0)’; 
end 
f(:,i)= state; 
m(:,i) = state; 
end 
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MATLAB Simulation of the Beam Expander Inertial Reference 
Unit for the Rapid Retargeting/Precision Pointing (R2/P2) 


simulation in seconds must be defined as the variable 
‘tmax’, the sampling interval is defined as ‘dt’. 


# 

# 

% 

# 

# 

4 Before running this simulation the length for the 
# 

# 

# This program generates a step to be input into 
# 

# 

4 


RCO UCCCCCCOCRRCCOCOCOCCOCCCOCCOLOSECLSE ECOL EERE SEER 20 


%sample rate for system 


x*ee Build Square Wave Input **** 


HEHHAAHAHAAA AHH T HARA RHR 
4 # 


# 
# 
# 
# 
4 
# 
# 
# 
# 
# 
# 
# 





t= 
ma 


BA << & of 0 & OL of 0P oP 0 of dP OP » DP HP AP AP OP AP OP OF AP AP OP OF 


for 
i 


e 
e 
e 
e 
e 
e 
f 


m 
end 





HAPPPR AAT HR RRR SR RRA A RPAH AARP R TR RRR RR AHR 
# 
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# # 
# # 
# MATLAB Simulation of the Beam Expander Inertial Reference {f# 
# Unit for the Rapid Retargeting/Precision Pointing (R2/P2) # 
4 System # 
# Before running this simulation the length for the # 
# simulation in seconds must be defined as the variable # 
¥ ‘tmax’, the sampling interval is defined as ‘dt’. # 
# This program generates a 10 Hz square wave to be 4 
# input into THESIST.M. # 
# § 
# # 


HHHFH RAHA SERA R RHR AHA ERRRARRRR HR RRR aH 


0.0005; tsample rate for system 
x=0.50/dt+1; 


xeek Build Square Wave Input **«*k 
zeros(2,kmax) ; 
zeros(5,kmax); 


i= l:kmax; 
f i<=100, 

state = (.005 .005 0 .005 0}j’; 
elseif i<=200 

state = -1*({.005 .005 0 .005 0j’; 
elseif i<=300 

state = [.005 .005 0 .005 Cj’; 
elseif i<=400 

State = -1*[.005 .005 0 .005 0)’; 
elseif i<=500 

state= (.005 .005 0 .005 0}’; 
lseif i<=600 

State = -1*[.005 .005 0 .005 0)’; 
lseif i<=700 

state= (.005 .005 0 .005 0)’; 
lseif i<=800 

state = -1*(.005 .005 0 .005 0)’; 
lseif i<=900 

state= [.005 .005 0 .005 0)’; 


lse 
state = -1*(.005 .005 0 .005 0)’; 
nd 
(:,i)= state; 
(:,i) = state; 








t= 
ma 


A< of Ww 0 CO, ow of oh OF OP CF OP WH OF DF Of GP OF OP OP CC IP OF AP OP 


for 
i 


e 
e 
e 
e 


e 


AMA a a aaa 
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# 

f 

# # 
# # 
f MATLAB Simulation of the Beam Expander Inertial Reference {4 
# Unit for the Rapid Retargeting/Precision Pointing (R2/P2) # 
# System # 
# Before running this simulation the length for the # 
# simulation in seconds must be defined as the variable # 
# ‘tmax’, the sampling interval is defined as ‘dt’. # 
# This program generates a 50 Hz square wave to be # 
# input into THESIST.M. # 
# # 
4 # 


HAHAHAHAHA Ra a a 


0.0005; %tsample rate for system 
x=0.10/dttl; 


keee Build Square Wave Input) **** 
zeros(2,kmax) ; 
zeros(5,kmax); 


i= 1:kmax; 
f i<=20, 

state = [.005 .005 0 .005 0}’; 
elseif i<=40 

state = -1*[.005 .005 0 .005 0)’; 
elseif i<=60 

state = [.005 .005 0 .005 0]’; 
elseif i<=80 

state = -1*(.005 .005 0 .005 0j’; 
elseif i<=100 

state= [{.005 .005 0 .005 0)’; 
lseif i<=120 

state = -1*(.005 .005 0 .005 0}’; 
lseif i<=140 

state= {.005 .005 0 .005 0)’; 
lseif i<=160 

state = -1*[.005 .005 0 .005 0)’; 
lseif i<=180 

state= [.005 .005 0 .005 0j’; 


lse 
state = -1*(.005 .005 0 .005 0)’; 
nd 
(:,i)= state; 
(:,1) = state; 
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t= 


PL OP A OP 8? DP FF OF OF OP OP OF OP OP OF OP dP OP OP OP OP 


cma 


A*< of oF vw 


for 
bi 


e 
e 
e 
© 


e 


e 
f 
m 
end 


HARTA RRR A ARASH 
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# 
# 
# # 
# # 
# MATLAB Simulation of the Beam Expander Inertial Reference 4 
f Unit for the Rapid Retargeting/Precision Pointing (R2/P2) # 
f System # 
f Before running this simulation the length for the # 
§ simulation in seconds must be defined as the variable # 
i] ‘tmax’, the sampling interval is defined as ‘dt’. # 
# This program generates a 100 Hz square wave to be # 
4 input into THESIST.M. + 
# # 
# 4 


PHHAGHHAR TH SRT H AES RA HAR ARR AHHH REE R HAAR Ree ede dae eda 


0.0005; %sample rate for system 
x=0.05/dt+1; 


e#ee* Build Square Wave Input *#*k* 
zeros(2,kmax); 
zeros(5,kmax) ; 


i= 1:kmax; 
f i<=10, 

state = [.005 .005 0 .005 0}’; 
elseif i<=20 

state = -1*{.005 .005 0 .005 0}’; 
elseif i<=30 

state = {.005 .005 0 .005 0}’; 
elseif i<=40 

state = -1*(.005 .005 0 .005 Oj)’: 
elseif i<=50 

state= [.005 .005 0 .005 0)’; 
lseif i<=60 

state = -1*{.005 .005 0 .005 0}’; 
lseif i<=70 

state= [(.005 .005 0 .005 0)’; 
lseif i<=80 

state = -1*{.005 .005 0 .005 0)’; 


lseif i<=90 
state= [.005 .005 0 .005 0}; 
lse 
state = -1*({.005 .005 0 .005 0}’; 
nd 
(:,1)= state; 
(:,1) = state; 





APPENDIX B: BODE PROGRAMS 


This appendix contains the programs used to compute the Bode diagrams 


contained in the main body of the thesis. 
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MATLAB simulation of Kalman Filter signal blending 
scheme. Developed by the Terry J Bauer, CPT USA, for 
the R2P2 fine tracking system. 


% 
t 
% 
t 
% 
% 
% 
$ 


y ‘delete thbode.met 
'delete tbode.met 
% 
% Enter the Transfer Functions For The Network 
% 
numm=(78.95 157.91); tMHD sensor 
denm=(1 12.57 157.91]; 
numg=(0 3947.8]; %Gyro sensor 
deng=(1 62.83 3947.8); 
w= logspace(-2,3); %frequency range 
% 
3 
% 
% belialial Enter Kalman Values ake 
% 
I = eye(5); 
wg = 10*2*pi; %Gyro break freq 
wm = 2*2*pi; %MHD break freq 
dat = .0005; Sample ratye 
A={({00000; 

0010 0; 
wg*2 -wg*2 -2*wg 0 0; 
‘ 00001; 


wm*2 0 0 -wm*2 -2*wm); 


B= (0 0 le-5 0 le-5)’; 

Q = (le-5)*2*I; 

R = ((1.237e-6)*2 0;0 (75e-6)%2); 
H=(01000;000 414 .5); 
{phi,del] = c2da(A,B,dt); 

L= (0010 1}’; 

D= {0 0;0 0;0 0;0 0;0 Oj; 


ake Bode for Kalman kkk 


Of) 0 dP oP oe 


= dlqe(phi,1,H,Q,R); 
[numk1l,denk]} ss2tf(phi+G*H,G,I,D,1); 
(numk2,denk2]j ss2tf£(phi+G*h,G,1I,D,2); 


% 

$ lalla! Combine Transfer Functions ak 

% 

nutl = conv(numg,numkl(1,:)) + conv(numm,numk2(1,:)); 
nut2 = conv(numg,numkl(2,:)) + conv(numm,numk2(2,:)); 
nut3 = conv(numg,numk1(3,:)) + conv(numm,numk2(3,:)); 
nut4 = conv(numg,numkl(4,:)) + conv(numm,numk2(4,:)); 
nutS = conv(numg,numk1i(5,:)) + conv(numm,numk2(5,:)); 

e ; det = conv(deng,denkl) + conv(denm,denk2); 


$nutl = numkl(1,:); 





tnut2 = numkl(2,:); 

$nut3 = numkl(3,:); 

tnut4 = numkl(4,:); 

tnutS = numkl(5,:); 

% 

% Calculate Bode Frequency Response 
% 


(magi,phasel 
(mag2,phase2 
[mag3,phase3 
{mag4, phase4 
({mag5,phase5 
clg 
semilogx(w,20*logl0(magl)),title(’KALMAN Blending Scheme (State X1)‘) 
Xlabel(’Frequency’) ,ylabel(’Magnitude (db)’),grid 

meta thbode 

pause 

tsemilogx(w,phasel),title(’KALMAN Blending Scheme (X1)’) 
txlabel(’Frequency’),ylabel(’Phase (deg)’),grid 

%pause 

meta 

semilogx(w,20*log1l0(mag2)),title(’KALMAN Blending Scheme (X2)’) 
xlabel(’Frequency’),ylabel(’Magnitude (db)’),grid 

meta 

pause 

tsemilogx(w,phase2),title(’KALMAN Blending Scheme(X2)’) 
txlabel(’Frequency’),ylabel(’Phase (deg)’),grid 

tmeta 

tpause 

semilogx(w,20*l0g10(mag3)),title(’KALMAN Blending Scheme(X3)’) 
Xxlabel(’Frequency’),ylabel{’Magnitude (db)’),grid 

meta 

pause 

$semilogx(w,phase3),title(’KALMAN Blending Scheme(X3) ’) 
%xlabel(’Frequency’),ylabel(’Phase (deg)’),grid 

tmeta 

tpause 

semilogx(w,20*1logl0(mag4)),title(’KALMAN Blending Scheme(X4) ’) 
xlabel(’Frequency’),ylabel(’Magnitude (db)’),grid 

meta 

pause. 

tsemilogx(w,phase4) ,title(’KALMAN Blending Scheme (X4)’) 
txlabel(’Frequency’) ,ylabel(’Phase (deg)’),grid 

%meta 

tpause 

semilogx(w,20*log10(mag5)),title(’KALMAN Blending Scheme(<5) ’) 
xlabel(‘’Frequency’),ylabel(’Magnitude (db)’),grid 

meta 

tpause 

tsemilogx(w,phase5),title(’KALMAN Blending Scheme (X5)‘) 

txlabel (‘Frequency’) ,ylabel('Phase (deg)’),grid 

tmeta 


bode(nutl,det,w); 
bode(nut2,det,w); 
bode (nut?,det,w); 
bode (nut4,det,w); 
bode(nut5,det,w); 


rare reny 
ioueouw wow 


THESBDE.M 1 NOV 89 
MATLAB simulation of Martin Marietta’s signal blending 


scheme. Developed by the R2/P2 Control Group in Denver, 
Colorado. 


Enter the Transfer Functions For The Network Ane Blender 


PP PP IP OP AR AP OP AP WP OP 


numm=(1 0 0}; %MHD sensor 
Asnm=(1 12.57 157.91); 

awammf=(1 0 0}; %MHD filter 
denmf=(1 62.8 3944); 

numg=(3947.8]); %Gyro sensor 
deng=(1 62.83 3947.8]; 

numgf=(157.8); Gyro filter 
dengf=({1 12.56 157.8); 

numbl=(398 30000 3.77e5 3.94e6]; tBlender 
denbl=[{1 1062.8 66744 3.94e6}; 

w= logspace(-2,3); frequency range 
% 


3 Combine Transfer Functions Along Branches 
% 

nummt=conv(numm,nummf) ; 

denmt=conv (denm,denmf) ; 

cl=conv(numg,numgf) : 

c2=conv (deng,dengf) : 

numgt=conv(cl,numb]) ; 
dengt=conv(c2,denbl1); 


ldifi= length(numgt) - length(nummt) ; 
if ldifl >=0 

nummt = [zeros(1,ldifl) nummt]); 

else 

numgt ={ zeros(1l,abs(ldifl)) numgt}; 

end 
ldif2= length(dengt) - length(denmt) ; 
if ldif2 >=0 

denmt = {zeros(1,ldif2) denmt]; 


else 
dengt =( zeros(l,abs(ldif2)) dengt]}; 
end 
numeq=nummt +numgt; tsum of the sensors 


deneq=denmt+dengt; 


Calculate Bode Frequency Response 


oP 0? oe 


(mag, phase]= bode(numeq,deneg,w) ; 

clg 

semilogx(w,20*logl0(mag)),title(’MMAG Blending Scheme’) 
xlabel(’Frequency’),ylabel(’Magnitude (db)’),grid 

meta tbode 

pause 

semilogx(w,phase),title(’MMAG Blending Scheme’) 
xlabel(’Frequency’),ylabel(’Phase (deg)’),grid 

meta 
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This program converts the transfer functions to state space 
reprensentations. It will then discretize the state space 
representation. 


0 AP IP dP OP WP SP OF AP HO 


% Enter transfer functions 
% 

numl=(0 0 3947.8); 

deni=(1 88.844 3947.8]; 

num2=(1 0 Oj; 

den2={1 12.57 157.91); 
{al,bl,cl,dlj=tf2ss(numl,denl); 
{a2,b2,c2,d2)=tf2ss(num2,den2) ; 


% 

% Convert to discrete time 
% 

dt=0.0005; 


[phi1l,del1]=c2d(al,b1,dt) ; 
[phi2,del2])=c2d(a2,b2,dt) ; 
$ 


% Find discrete 5x5 for whole system 
% 
a=(0 1000 

~3947.8 ~88.844 00 0 

00010 

0 0 -157.91 -12.57 0 

0 0 0 0 -1000); 


% 
b=(0 -3947.8 0 -157.91 1000]’; 
t 
{ 


phi,delj=c2d(a,b,dt) ; 


phit=phil+phi2; 
delt=dell+del2; 
at=al+a2; 
bt=bl+b2; 
ct=cl+c2; 
dt=d1l+d2; 


ny 
ta 
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